#include <xc.h>
#include "Pin.h"
#include "Enumerations.h"
#include <sys/attribs.h>

Pin::Pin() { }

Pin::Pin(Enums::Gpio::Ports port, Enums::Gpio::Pins::Number number)
{
	this->SetPort(port);
	this->Number = number;
	this->SetDirection(Enums::Gpio::Pins::Direction::Input);
}

Pin::Pin(Enums::Gpio::Ports port, Enums::Gpio::Pins::Number number, Enums::Gpio::Pins::Direction direction)
{
	this->SetPort(port);
	this->Number = number;
	this->SetDirection(direction);
}

Pin::~Pin() { }

void Pin::SetPort(Enums::Gpio::Ports port)
{
	this->Port = port;
}

void Pin::SetDirection(Enums::Gpio::Pins::Direction direction)
{
	this->Direction = direction;

	// Sets the direction of the pin as an input (1) or output (0)
	switch (this->Port)
	{
		case Enums::Gpio::Ports::PortA:
			TRISA = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISA | (1 << (UINT16)this->Number) :
					TRISA & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortB:
			TRISB = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISB | (1 << (UINT16)this->Number) :
					TRISB & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortC:
			TRISC = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISC | (1 << (UINT16)this->Number) :
					TRISC & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortD:
			TRISD = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISD | (1 << (UINT16)this->Number) :
					TRISD & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortE:
			TRISE = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISE | (1 << (UINT16)this->Number) :
					TRISE & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortF:
			TRISF = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISF | (1 << (UINT16)this->Number) :
					TRISF & ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortG:
			TRISG = Direction == Enums::Gpio::Pins::Direction::Input ?
					TRISG | (1 << (UINT16)this->Number) :
					TRISG & ~(1 << (UINT16)this->Number);
			break;
	}

}

void Pin::SetPin()
{
	// Sets the appropriate latch (LATx) bit for the pin
	switch (this->Port)
	{
		case Enums::Gpio::Ports::PortA:
			LATA |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortB:
			LATB |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortC:
			LATC |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortD:
			LATD |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortE:
			LATE |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortF:
			LATF |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortG:
			LATG |= (1 << (UINT16)this->Number);
			break;
	}
}

void Pin::ClearPin()
{
	// Clears the appropriate latch (LATx) bit for the pin
	switch (this->Port)
	{
		case Enums::Gpio::Ports::PortA:
			LATA &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortB:
			LATB &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortC:
			LATC &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortD:
			LATD &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortE:
			LATE &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortF:
			LATF &= ~(1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortG:
			LATG &= ~(1 << (UINT16)this->Number);
			break;
	}
}

void Pin::TogglePin()
{
	// Toggles the appropriate latch (LATx) bit for the pin
	switch (this->Port)
	{
		case Enums::Gpio::Ports::PortA:
			LATA ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortB:
			LATB ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortC:
			LATC ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortD:
			LATD ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortE:
			LATE ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortF:
			LATF ^= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Ports::PortG:
			LATG ^= (1 << (UINT16)this->Number);
			break;
	}
}

Enums::Gpio::Pins::Value Pin::GetPinValue()
{
	UINT16 PortState;

	// Read the entire PORTx
	switch (this->Port)
	{
		case Enums::Gpio::Ports::PortA:
			PortState = PORTA;
			break;
		case Enums::Gpio::Ports::PortB:
			PortState = PORTB;
			break;
		case Enums::Gpio::Ports::PortC:
			PortState = PORTC;
			break;
		case Enums::Gpio::Ports::PortD:
			PortState = PORTD;
			break;
		case Enums::Gpio::Ports::PortE:
			PortState = PORTE;
			break;
		case Enums::Gpio::Ports::PortF:
			PortState = PORTF;
			break;
		case Enums::Gpio::Ports::PortG:
			PortState = PORTG;
			break;
	}

	// Return HIGH if bit is set or LOW if it is not
	return (PortState & (UINT16) (1 << (UINT16)this->Number)) == 0 ? Enums::Gpio::Pins::Value::High : Enums::Gpio::Pins::Value::Low;
}

void Pin::EnableChangeNotification(Enums::Gpio::Pins::PullUpState pullUpState, Enums::Interrupts::Priority priority, Enums::Interrupts::Subpriority subpriority, bool stopOnIdle)
{
	UINT32 changeNotificationValue = 0;
	// Verify PORT and PIN combination are valid for Change Notification
	do
	{
		// CN0
		if ((this->Port == Enums::Gpio::Ports::PortC) && (this->Number == Enums::Gpio::Pins::Number::Pin14))
		{
			changeNotificationValue = (1 << 0);
			break;
		}
		// CN1
		if ((this->Port == Enums::Gpio::Ports::PortC) && (this->Number == Enums::Gpio::Pins::Number::Pin13))
		{
			changeNotificationValue = (1 << 1);
			break;
		}
		// CN2
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin0))
		{
			changeNotificationValue = (1 << 2);
			break;
		}
		// CN3
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin1))
		{
			changeNotificationValue = (1 << 3);
			break;
		}
		// CN4
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin2))
		{
			changeNotificationValue = (1 << 4);
			break;
		}
		// CN5
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin3))
		{
			changeNotificationValue = (1 << 5);
			break;
		}
		// CN6
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin4))
		{
			changeNotificationValue = (1 << 6);
			break;
		}
		// CN7
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin5))
		{
			changeNotificationValue = (1 << 7);
			break;
		}
		// CN8
		if ((this->Port == Enums::Gpio::Ports::PortG) && (this->Number == Enums::Gpio::Pins::Number::Pin6))
		{
			changeNotificationValue = (1 << 8);
			break;
		}
		// CN9
		if ((this->Port == Enums::Gpio::Ports::PortG) && (this->Number == Enums::Gpio::Pins::Number::Pin7))
		{
			changeNotificationValue = (1 << 9);
			break;
		}
		// CN10
		if ((this->Port == Enums::Gpio::Ports::PortG) && (this->Number == Enums::Gpio::Pins::Number::Pin8))
		{
			changeNotificationValue = (1 << 10);
			break;
		}
		// CN11
		if ((this->Port == Enums::Gpio::Ports::PortG) && (this->Number == Enums::Gpio::Pins::Number::Pin9))
		{
			changeNotificationValue = (1 << 11);
			break;
		}
		// CN12
		if ((this->Port == Enums::Gpio::Ports::PortB) && (this->Number == Enums::Gpio::Pins::Number::Pin15))
		{
			changeNotificationValue = (1 << 12);
			break;
		}
		// CN13
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin4))
		{
			changeNotificationValue = (1 << 13);
			break;
		}
		// CN14
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin5))
		{
			changeNotificationValue = (1 << 14);
			break;
		}
		// CN15
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin6))
		{
			changeNotificationValue = (1 << 15);
			break;
		}
		// CN16
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin7))
		{
			changeNotificationValue = (1 << 16);
			break;
		}
		// CN17
		if ((this->Port == Enums::Gpio::Ports::PortF) && (this->Number == Enums::Gpio::Pins::Number::Pin4))
		{
			changeNotificationValue = (1 << 17);
			break;
		}
		// CN18
		if ((this->Port == Enums::Gpio::Ports::PortF) && (this->Number == Enums::Gpio::Pins::Number::Pin5))
		{
			changeNotificationValue = (1 << 18);
			break;
		}
		// CN19
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin13))
		{
			changeNotificationValue = (1 << 19);
			break;
		}
		// CN20
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin14))
		{
			changeNotificationValue = (1 << 20);
			break;
		}
		// CN21
		if ((this->Port == Enums::Gpio::Ports::PortD) && (this->Number == Enums::Gpio::Pins::Number::Pin15))
		{
			changeNotificationValue = (1 << 21);
			break;
		}
		return;
	} while (true);

	// Disable CPU Interrupts
	asm volatile ("di"); // Disable All Interrupts
	asm volatile ("ehb"); // Stop instruction execution until execution hazards are cleared

	// Change pin direction to an input
	this->SetDirection(Enums::Gpio::Pins::Direction::Input);

	// Enable CN Module
	CNCONbits.ON = 1;
	CNCONbits.SIDL = stopOnIdle ? 1 : 0;

	// Configure Pull Up/Down state
	switch (pullUpState)
	{
		case Enums::Gpio::Pins::PullUpState::PullUp:
			CNPUE |= (1 << (UINT16)this->Number);
			break;
		case Enums::Gpio::Pins::PullUpState::PullDown:
			break;
		case Enums::Gpio::Pins::PullUpState::Disabled:
			CNPUE &= ~(1 << (UINT16)this->Number);
			break;
	}

	// Enable CN Pin
	CNEN |= changeNotificationValue;

	// Read PORT to clear pin mismatch
	this->GetPinValue();

	// Configure Priority
	IPC6bits.CNIP = (BYTE)priority;
	IPC6bits.CNIS = (BYTE)subpriority;

	// Clear CN Interrupt Flag
	IFS1bits.CNIF = 0;

	// Enable CN Interrupt
	IEC1bits.CNIE = 1;

	// Enable CPU Interrupts
	asm volatile ("ei"); // Enable All Interrupts
	asm volatile ("ehb"); // Stop instruction execution until execution hazards are cleared

}

/*! \brief Change Notification Interrupt
 */
extern "C"
void __ISR(_CHANGE_NOTICE_VECTOR, ipl2) GpioChangeNotification_OnPinChanged(void)
{
	// Clear CN Interrupt Flag
	IFS1bits.CNIF = 0;
	
	// Place Interrupt Handler Code Here
}
